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APPLICATION  OF  THE  KALMAN  FILTER  TO  SPHERICAL 
DISCRETE  TRACK  SMOOTHING  AND  PREDICTION 

1.0  Introduction 

The  Kalman  filter  is  capable  of  estimating  the  state  variables 
of  a  linear  dynamic  system  under  conditions  of  random  perturbation  of 
that  system  and  noisy  observation  of  a  linear  combination  or  combina- 
tions of  the  state  variables.   The  motion  of  an  airborne  target,  though 
presumably  describable  by  linear  dynamic  equations  in  a  cartesian  frame, 
is  most  often  observed  in  a  spherical  coordinate  frame,  as  is  the  case 
in  radar  tracking.   The  transformation  from  cartesian  to  spherical  co- 
ordinates is  of  course  a  nonlinear  transformation,  and  does  not  allow 
the  formulation  of  a  constant  observability  matrix  as  required  to  the 
implementation  of  the  Kalman  estimation  scheme. 

This  paper  investigates  the  effectiveness  of  a  method  for  cir- 
cumventing the  problem  of  nonlinear  observation  of  a  linear  system's 
state  variables.   The  method  employs  a  linearization  of  the  cartesian- 
to-spherical  coordinate  transformation.   The  performance  of  a  filter 
designed  by  this  method  is  observed  as  a  function  of  measurement 
error,  accuracy  of  filter  initialization,  and  the  relevance  of  the 
estimated  initial  error  covariance,  P„  i  , . 

The  investigation  reveals  that  the  most  critical  factor  in  fil- 
ter performance  and  stability  is  filter  state  initialization.   The 
filter  performs  remarkably  well,  even  in  the  face  of  considerable  mea- 
surement error,  provided  it  gets  a  "good  start",  i.e.,  a  fairly  ac- 
curate initialization  of  the  filter  states.   The  linearization  scheme 
yields  instability,  however,  for  poor  initialization.   This  is  rather 


unfortunate  since  in  a  realistic  situation  the  accuracy  of  initiali- 
zation is  a  direct  function  of  measurement  error.   The  results  under- 
score the  need  for  more  powerful  initialization  schemes  for  use  in 
this  and  related  nonlinear  filtering  problems. 

Included  are  digital  computer  simulations  of  filter  performance. 
2.0  Linearization  of  the  Coordinate  Transformation 

Figure  1  represents  in  block  diagram  form  the  discrete  state  tran- 
sition equations  for  the  dynamic  process  and  the  Kalman  filter.   All 
ooex  at  ions  .within  the  dotted  contour  are  carried  out  in  cartesian  co- 
ordinates.  The  observability  transformation  operator  T  is  the  non- 
linear cartesian-to-spherical  coordinate  transformation.   The  filter 
gain  matrix  G  assumes  and  in  fact  requires  for  its  calculation  a 
linear  observability  matrix  H.   Such  a  matrix  is  obviously  absent  in 
the  present  problem. 

The  linearization  proceeds  from  the  observation  that  the  quan- 
tity 

x  =  x   -  X  |  (1) 

-k   ~*k   — k  I  k-1 

is  the  difference  between  two  hopefully  similar  quantities,  and  may 
thus  be  classified  as  a  "small  difference".   Dropping  the  subscripts 
from  equation  (1)  for  convenience  and  re-arranging, 

x  =  x  -  2£  (2) 

We  now  note  that 

£  =  T  [  £  ]  =  T  [  x  -  x  ]  (3) 

A  Taylor  series  expansion  of  equation  (3)  about  x  yields 

z=T[xl-r-   x+ (A) 
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Figure  1.  Block  diagram  of  the  dynamic  process  and  the 
Kalman  filter  estimating  the  state  variables 
of  that  process. 


where  the  higher-order  derivatives  have  been  dropped.   We  may  also 
write 

z  =  z  -  ft  (5) 

Noting  that 

z  =  T  [  x  ]  +  v  (6) 

equation  (5)  combined  with  (4)  will  give 

St 


z  =  T 


[x]+v-{t[x]-;j-|  x  I 

—  X 


or 


:  =  v  +  —  |  x  (7) 


s*** 


The  role  of  the  partial  derivative  in  equation  (7)  is  seen  by  com- 
parison with  the  corresponding  equation  for  linear  observability, 

namely 

~  A 

£  =  Hx  +  v-Hx 

~  A  ^ 

or  z   =  v  +  H(x  -  x)  =  v  +  H  x  (8) 

The  partial  of  T  with  respect  to  x,  evaluated  at  x  rather  than  x  be- 
cause the  latter  is  not  available,  may  now  be  used  to  fill  the  role 
of  H  in  the  calculation  of  the  filter  gain  matrix  G  for  each  sample. 

3.0  Filter  Simulation 

3.1  Target  Track  and  Dynamics 

The  true  target  information  that  follows  is  common  to  all  subse- 
quent examples.   It  assumes  the  tracking  radar  to  be  carried  by  an 
airborne  interceptor. 


Initial  target  position    North  60  miles 

East   10  miles 
Down   2  miles 
Initial  target  heading     180° 
Interceptor  heading        000   throughout 
Initial  target  velocity    0.1473  miles  per  second 
Interceptor  velocity       0.148  miles  per  second,  throughout 
The  target  executes  the  following  maneuver: 

at  t  =  106  seconds,  acceleration  as  follows 
North  +0.001715  miles  per  second 
East   -0.00474  miles  per  second^ 
This  acceleration  continues  for  twenty  seconds,  at  which  time  the  tar- 
get is  headed  220°  with  a  velocity  of  0.1473  miles  per  second.   The 
duration  of  the  run  is  180  seconds. 

Observations  are  taken  every  two  seconds.   The  target  dynamics 
in  each  of  the  three  cartesian  coordinates  have  been  represented  by 

—  ,  with  all  three  motions  assumed  uncoupled. 
s^ 

No  attempt  was  made  either  to  estimate  or  to  sense  target  maneu- 
vers.  The  filter  was  kept  "open"  to  the  possibility  of  maneuvers  by 
assigning,  for  purposes  of  filter  gain  calculation,  an  arbitrarily 
small  variance  to  the  random  disturbance  w,  .   The  resulting  non-zero 
Q  matrix,  where 

Q  =  *   E  [Wk  ]  ^  (9) 

effectively  prohibits  the  convergence  of  filter  gains  to  zero,  and 
hence  keeps  active  the  link  between  the  target  and  the  filter. 


At  each  observation,  it  was  necessary  to  solve,  in  addition  to 
the  conversion,  dynamic,  smoothing  and  prediction  equations  implied  by 
Figure  1,  each  of  the  following  equations  for  determination  of  the  fil- 
ter gains: 


1  <  [\  Pk  I  k-1  ^  +  R] 


Gu  =  K  i  t  ,  K  \K  p,.  i,_  ,  K  +  R  (10> 


k  I  k-i  °k  L"k  M  k 


pi    i  l  ,    -  0    p,   I  i    i   ~  G,    M,    p,   I  i    i    I  0T  +  Q 
k+l     k  Lk     k-1  k     k     k     k-1   J 


(11) 


P,  l  ,  ,  is  the  covariance  matrix  of  prediction  error.   The  M  matrix  is 
k  |  k-1  k 

found  as 

M  =|I|  (12) 

k   ax  a   A 

^  =  ^klk-l 

and  must  be  evaluated  at  every  observation.   R  is  simply  the  covariance 

matrix  of  spherical  observation  noise. 

3.2  Example  1 

In  example  1,  the  radar  observables  are  range,  range  rate,  and  the 

two  angles  9  and  0,  as  depicted  in  Figure  2.   The  standard  deviations  for 

spherical  observation  noise  are  as  follows: 

(7      =  1.437  miles 
range 

afl     =  0.0035  radians 
(7      =  0.0174  radians 

0 

G.  =   0.00152  miles/second 

r 

Linearization  of  the  coordinate  transformation 


=  yN2  +  Ez  +  D2  (i3) 


Q  =  tan"1  £  (14) 
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Figure  2.   Spherical  coordinates  for 
Example  1. 
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where  a  =  NN  +  EE  +  DD.   The  filter  cartesian  state  variables  were  in- 
itialized by  inversion  of  the  coordinate  transformation  through  velocity, 
using  first  differences  from  the  initial  two  observations  where  necessary. 
Although,  as  will  be  shown,  the  initialization  and  bootstrapping  employed 
in  this  example  failed  in  some  cases  to  yield  a  stable  track,  it  is  now 
described  in  some  detail  as  an  illustration  of  the  difficulties  en- 
countered in  initialization.   The  observations  are  numbered  starting 
with  1.   The  following  expressions  describe  the  starting  procedure.  The 
variables  are  as  shown  in  Figure  1. 

a)  Observation  1  yields   z 

-1 

b)  Observation  2  yields   z 

~2 

c)  Inverse  coordinate  transformation  and  first  differences  on  ob- 
servations 1  and  2  yield  position  and  velocity  in  all  three 
cartesian  coordinates.   Accelerations  are  arbitrarily  set  to 
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zero.   The  filter  cartesian  states  x     are  now  set  to  these 

~2  I  2 

values. 

d)  x  ,   is  calculated 
"3  I  2 

e)  z  -   is  calculated 

"3  I  2 

St 

f)  M  =  —  I     *    is  calculated 

2   ox  x  =  x 

"   ~2  I  2 

g)  G  and  P     are  calculated  as  in  equations  (10)  and  (11), 

2      3  I  2 

using  P    ,  the  initializing  value  of  prediction  error  co- 

2  I  1 
variance,  and  M  .   It  would  appear  that  a  further  smoothing 

of  filter  states  would  now  be  possible,  since  a  filter  gain 

matrix  G  has  been  calculated.  We  note,  however,  that  there 
2 

■*  A         A 

has  not  been  generated  an  x  upon  which  x     and  z  .   de- 

"1  I  1  "2  I  1     "2  I  1 

pend.   Without  the  latter  we  cannot  generate  ~  =  z   -  z 

"2   ~2   ~2  I  1 

It  is  for  this  reason  that  the  first  smoothing  is  deferred  to 
the  third  observation. 


=  *1 
ox' 

3  I  2 


1.)  m.  -£!,    . 


i)   G  and  P  .   are  calculated  from  P  .   and  M 
3      4  I  3  3  I  2      3 

j)   Observation  3  yields  z_ 

*       A  A 

k)   Smoothing  occurs,  x,   =x     +  G   (z   -  z  ,  ) 

"3  I  3  -3 1  2        3-3     -3 1  T 

Bootstrapping  is  now  complete,  and  the  filter  operates  from  this  point 
on  in  a  straight  forward  fashion. 

It  is  not  implied  and  not  to  be  inferred  that  this  initialization  and 


bootstrapping  scheme  is  more  effective  or  more  desirable  than  any  one 

of  many  other  such  methods.   Serious  questions  can,  in  fact,  be  raised 

about  the  fact  that  the  initializing  value  P  ,   is  not  the  one  used 

2  |  1 

in  the  first  smoothing.   The  first  filter  gain  used,  G  ,  is  based  upon 

P  i   ,  a  calculated  value.   It  is  for  these  reasons  that  subsequent 

3  I  2  H 

examples  will  attempt,  insofar  as  possible,  to  isolate  the  effects  of 
obervation  noise,  error  covariance  initialization,  and  filter  state 
initialization. 

Returning  now  to  example  1,  there  remains  only  to  note  that  P_  i 
was  chosen  as 

5 


2   1 


(18) 


The  raw  radar  data  and  the  smoothed  track  for  the  NE  plane,  both 
compared  with  the  true  track  (as  seen  from  the  interceptor)  are  shown 
in  Figure  3.   This  run  was  chosen  as  example  1  because  it  typifies 
the  transition  from  stable  to  unstable  filter  performance  for  the 
initialization  scheme  outlined  above.   The  smoothed  track  shows 
evidence  of  indecision  in  the  early  stages,  an  indecision  that  be- 
comes instability  for  higher  observation  noise  levels.   The  filter 
is  observed  to  be  quite  effective  for  the  better  part  of  the  track. 
Runs  performed  at  lower  noise  yielded  virtually  immediate  lock-on 
to  a  very  satisfactory  smoothed  track. 
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3.3  Example  2 

Example  2  seeks  to  isolate  the  effect  of  measurement  noise  upon 
filter  performance.   The  first  step  taken  here  was  to  bypass  the  boot- 
strapping procedure.   This  was  accomplished  by  starting  the  program 
at  step  h)  in  the  bootstrap  outline  given  above,   x  i   was  set  equal 

to  the  true  x  ,  while  P~  i  ~,  the  covariance  of  prediction  error  asso- 
-3        3  |  2' 

ciated  with  x  ,   ,  was  properly  set  to  zero.   Hence  the  filter  is 
-3  |  2 

getting  not  only  an  error-free  start  with:  respect  to  the  first  set  of 
predicted  filter  states,  but  a  precise  corresponding  conditional  co- 
variance  initialization  as  well.   This  effectively  isolates  measure- 
ment noise  as  a  variable  in  the  problem.   The  initial  value  of  zero 
for  covariance  acts  to  effectively  shut  off  filter  gains  until  the  P 
matrix  starts  accumulating  value  from  the  nominally  small  value  of  Q. 
This  process  of  growth  for  the  P  and  G  matrices  is  readily  seen  by 
inspection  of  equations  (10)  and  (11).   The  low  initial  values  of  G 
allow  the  error-free  initial  filter  states  to  proceed  very  nearly 
along  the  true  track,  virtually  free  of  any  correction  arising  from 
observation  noise,  until  such  time  as  P  has  grown  to  an  appreciable 
level . 

Nine  runs  are  presented  in  this  example.   In  the  first  run,  the 
measurement  noise  standard  deviations  are  one-tenth  of  the  corres- 
ponding values  in  example  1.   In  each  of  the  eight  subsequent  runs,  the 
standard  deviations  are  increased  by  a  factor  of  2.   Figure  4  shows  the 
set  of  9  smoothed  tracks,  shown  with  the  true  track,  for  the  NE  plane. 
The  corresponding  raw  observed  tracks  are  not  included,  but  an  apprecia- 
tion for  these  tracks  might  be  obtained  by  noting  that  the  raw  track 

11 


True 


Raw 


Filtered 


Figure  3.   True,  raw,  and  filtered  tracks,  NE  plane,  for  Example  1 
Scale:   7  miles  per  inch. 
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of  Figure  3,.  example  1,  would  fit  roughly  between  the  raw  tracks 
underlying  smoothed  tracks  4  and  5  of  Figure  4.   We  may  further  note 
that  the  raw  track  for  run  9,  Figure  4,  is  roughly  25  times  as  noisy 
as  that  shown  in  Figure  3a. 

We  can  only  conclude  from  the  results  of  example  2  that  an  ever- 
increasing  level  of  measurement  noise  is  not  of  itself  sufficient  to 
render  the  filter  unstable,  at  least  in  the  case  of  error-free  state 
and  covariance  initialization.   This  tends  to  indict  filter  state  in- 
itialization and/or  conditional  covariance  initialization  as  the  weak 
points  leading  to  instability.   This  is  looked  into  in  example  3. 
3.    Example  3 

In  example  3,  observation  noise  levels  are  identical  for  each  of 
seven  runs  and  are  equal  to  those  given  in  example  1.   The  starting 
bootstrap  was  once  again  bypassed.   In  the  first  run  the  filter  was 
given  an  error-free  start,  (x~  i  ?  =  x-i)  >  an<*  P3  I  2  was  accordingly  set 
equal  to  zero.   In  the  subsequent  six  runs,  increasingly  larger  state 
initialization  errors  were  introduced,  while  concurrently  P~  l  ~  was 
incremented  to  accurately  reflect  this  increase.   In  run  2,  P  1   was 
set  to  four-hundredths  of  the  value  shown  in  equation  (18)  and  was  in 
each  subsequent  run  increased  by  a  factor  of  four.   In  runs  2  through 
7,  the  random  number  generation  subroutine,  serving  as  a  source  of 
error  for  both  initialization  and  measurement  noise,  was  re-initialized 
so  as  to  produce  for  each  run  the  same  base  state  initialization  error 
vector  and  the  same  sequence  of  measurement  errors.   Hence  all  state 
initializations  lie  on  the  same  vector,  differing  only  in  magnitude. 
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Figure  4.   True  and  filtered  tracks,  NE  plane,  for  Example  2, 
Scale:   7  miles  per  inch 


14 


Figure  5  shows  the  smoothed  track  in  the  NE  plane  for  all  seven 
runs  of  example  3.   The  raw  track  for  each  run  is  identical  to  that  of 
Figure  3.   The  true  track  is,  of  course,  also  identical  to  that  of 
Figure  3. 

We  may  conclude  from  the  results  of  example  3  that  accurate  co- 
variance  initialization  does  not  of  itself  remove  the  possibility  of 
filter  instability. 

State  initialization,  then,  appears  to  be  especially  critical. 
3.5  Example  4 

One  final  example  was  chosen  to  investigate  the  possibility  that 
the  fixed  direction  of  the  initial  state  error  vector  of  example  3 
might  have  been  a  misleading  factor  in  arriving  at  the  conclusions 
drawn.   Nine  runs  were  made  in  example  4,  with  all  conditions  identi- 
cal to  those  of  run  4  in  example  3  except  for  the  fact  that  the  ran- 
dom number  subroutine  was  not  re-initialized  for  each  run.   This  pro- 
duced the  ensemble  of  smoothed  tracks  for  the  NE  plane,  shown  in  Fig- 
ure 6.   These  tracks  have  been  grouped  to  show  those  considered  good, 
poor,  and  unstable. 

We  may  conclude  from  example  4  that  although  the  covariance 
initialization  may  accurately  reflect  the  statistical  properties  of 
initial  state  errors,  individual  samples  of  the  latter  may  be  quite 
far  removed  from  the  mean,  adding  another  degree  of  sensitivity  to 
the  initialization  problem.   No  effort  was  made  to  establish  a  cor- 
relation between  the  tracks  of  Figure  6  and  the  extent  of  dispersion 
in  the  nine  different  initial  state  error  vectors  drawn  from  the  same 
Gaussian  distribution. 
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Figure  5.   Filtered  tracks,  NE  plane, 
for  Example  3. 

Scale:   7  miles  per  inch 
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Unstable 

Figure  6.   Filtered  tracks,  NE  plane,  for  Example  4. 
Scale:   7  miles  per  inch. 
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4.0  Conclusion 

Filter  state  initialization  appears  to  be  the  most  critical  fac- 
tor in  filter  performance  and  stability.   A  bootstrap  starting  pro- 
cedure, to  be  effective,  must  place  the  initial  state  vector  within 
certain  bounds.   Although  no  algorithm  is  presented  for  analytically 
determining  these  bounds,  their  existence  is  evident  in  Figure  5, 
where  filter  performance  is  seen  to  deteriorate  starting  with  run  four, 
The  bounds  are  very  likely  strongly  dependent  upon  measurement  noise 
levels . 

An  effective  bootstrap  procedure  must  also  include  the  means  of 
determining  meaningfully  all  elements  of  the  initial  error  covariance 
matrix.   This  is  not  a  simple  task,  mainly  because  nonlinear  observ- 
ability leads  to  dependence  between  initial  state  estimates,  and  hence 
a  full  initial  P  matrix. 
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